#include "function.h"

functiont::functiont() {}

functiont::~functiont() {}

//这样写就所有类的实例都是指向唯一对象了
functiont * functiont::instance(void)
{
    static functiont obj;
    return &obj;
}

double functiont::r2d(double Radius)
{
    double ret = (Radius / PI) * 180;
    return ret;
}

void functiont::AxisPowerONALL()
{
    for(int i=0;i<RobotAxisNum;i++)
    {
        functiont::instance()->RobotHardwareInfo.TargetPosition.DATA[i]=functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[i];
    }

    functiont::instance()->DoPowerONIndex=-1;
    functiont::instance()->job=1;
}

void functiont::AxisPowerOFFALL()
{
    functiont::instance()->DoPowerONIndex=-1;
    functiont::instance()->job=-1;

}

void functiont::AxisClearErrorALL()
{
    //自己也要清除错误

}

void functiont::AxisPowerONSingle(int axis)
{
    //上使能之后先把全局变量改一下数值
    functiont::instance()->RobotHardwareInfo.TargetPosition.DATA[axis]=functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[axis];

    functiont::instance()->DoPowerONIndex=axis;

    functiont::instance()->job=1;
}

void functiont::AxisPowerOFFSingle(int axis)
{
    functiont::instance()->DoPowerONIndex=axis;
    functiont::instance()->job=-1;

}



//将输入的数组形式的关节位置转换成字符串形式
string functiont::ChangeJointArrayToString(double* JointDATA,int RealNum,int Percision)
{
    std::ostringstream ret;
    ret << "[";
    for (int i = 0; i < RealNum; ++i) {
        ret << std::fixed << std::setprecision(Percision) << JointDATA[i];
        if (i < RealNum - 1) {
            ret << ", ";
        }
    }
    ret << "]";
    return ret.str();
}

//对于输入的的字符串形式的变量，需要做一些预处理，将字符串转换成数组
void functiont::parseStringToDoubleArray(const QString& str, double result[4]) {
    // 初始化数组为 0
    for (int i = 0; i < 4; ++i) {
        result[i] = 0.0;
    }

    // 去掉字符串中的 '[' 和 ']'
    QString trimmedStr = str;
    trimmedStr.remove('[');
    trimmedStr.remove(']');

    // 按逗号分割字符串
    QStringList strList = trimmedStr.split(",");

    // 将字符串转换为 double 并存储到数组中
    for (int i = 0; i < 4 && i < strList.size(); ++i) {
        result[i] = strList[i].toDouble();
    }
}



//获取本机IP
NetWorkInfo functiont::GetTargetIPInfo(int index)
{
    NetWorkInfo ret=NetWorkInfo();
    ret.isValid=false;

    QList<QNetworkInterface> list=QNetworkInterface::allInterfaces();//网络接口的列表
    for(int i=0;i<list.count();i++)
    {
        QNetworkInterface aInterface=list.at(i);
        if(!aInterface.isValid())
            continue;

        //        std::cout<<"设备名称："<<aInterface.humanReadableName().toStdString()<<std::endl;
        //        std::cout<<"硬件地址："<<aInterface.hardwareAddress().toStdString()<<std::endl;

        if(index==i)
        {
            ret.Name=aInterface.humanReadableName();
            ret.MAC=aInterface.hardwareAddress();
        }

        QList<QNetworkAddressEntry> entryList=aInterface.addressEntries();
        for(int j=0;j<entryList.count();j++)
        {
            QNetworkAddressEntry aEntry=entryList.at(j);

            //            std::cout<<"IP地址："<<aEntry.ip().toString().toStdString()<<std::endl;
            //            std::cout<<"子网掩码："<<aEntry.netmask().toString().toStdString()<<std::endl;
            //            std::cout<<"广播地址："<<aEntry.broadcast().toString().toStdString()<<std::endl;

            //同一个网卡包括IPV4和IPV6的信息，我们只提取IPV4的
            //192.168.146.143
            //fe80::3901:697e:ba51:61e9%enp2s1
            if(!aEntry.ip().toString().contains("%"))
            {
                ret.isValid=true;
                ret.IP=aEntry.ip().toString();
                ret.netmask=aEntry.netmask().toString();
                ret.broadcast=aEntry.broadcast().toString();
            }

        }

    }

    return ret;
}

void functiont::timerInit(int InputTick)
{
    timer->setInterval(InputTick);
}

void functiont::timerStart()
{
    timer->start();
}

double functiont::ChangeEncoderToRealValue_Pos(double EncoderValue,int EncoderIndex)
{
    return EncoderValue/joint_gearratio[EncoderIndex];
}

double functiont::ChangeRealToEncoderValue_Pos(double RealValue,int EncoderIndex)
{
    return RealValue*joint_gearratio[EncoderIndex];
}

double functiont::d2r(double Degree)
{
    double ret= (Degree / 180)*PI;
    return ret;
}

//机器人的死循环(类似于PLC的处理方法)
void functiont::MAINLOOP()
{
    double NewAddAbsoluteArrayTEST[4];

    if(true)
    {
        for(int i=0;i<RobotAxisNum;i++)
        {
            CurrentPositionReal[i]=joint_direction[i]*ChangeEncoderToRealValue_Pos(functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[i],i) + joint_direction[i]*ChangeEncoderToRealValue_Pos(joint_calibrateBias[i],i);
            CurrentPositionRealRadius[i]=functiont::d2r(CurrentPositionReal[i]);
            StatusWord.DATA[i]=functiont::instance()->RobotHardwareInfo.Statusword.DATA[i];
            ErrorCode.DATA[i]=functiont::instance()->RobotHardwareInfo.Errorcode.DATA[i];

            if(StatusWord.DATA[i]==0)
            {
                StatusWordString.DATA[i]="NONE";
            }


            if(ErrorCode.DATA[i]==0)
            {
                ErrorCodeString.DATA[i]="NONE";
            }
            //when robot is moving will not update theRadius!!!
            if(job<2)
            {
                //the.DATA[i]=CurrentPositionReal[i];
                theRadius[i]=CurrentPositionRealRadius[i];

            }
        }
        //std::cout<<"MAINLOOP theradius[0] : "<<theRadius[0]<<",  theradius[1] : "<<theRadius[1]<<",  theradius[2] : "<<theRadius[2]<<",  theradius[3] : "<<theRadius[3]<<std::endl;
    }
    if(job == 2){
        for(int i=0;i<RobotAxisNum;i++)
        {
            NewAddAbsoluteArrayTEST[i]=joint_direction[i]*functiont::r2d(tempVector[i]);
            TargetPositionfinal[i]=ChangeRealToEncoderValue_Pos(NewAddAbsoluteArrayTEST[i],i)-joint_calibrateBias[i];

            functiont::instance()->RobotHardwareInfo.CurrentPosition.DATA[i]=TargetPositionfinal[i];
            functiont::instance()->RobotHardwareInfo.TargetPosition.DATA[i]=TargetPositionfinal[i];

        }
        job = 0;
    }

}
